#!/usr/bin/env python
#################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#################################################################################

# Authors: Gilbert #

import rospy
import random
import time
import os
from gazebo_msgs.srv import SpawnModel, DeleteModel
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import Pose

class Respawn():
    def __init__(self):
        self.modelPath = os.path.dirname(os.path.realpath(__file__))
        self.modelPath = self.modelPath.replace('turtlebot3_machine_learning/turtlebot3_dqn/src/turtlebot3_dqn',
                                                'turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_square/goal_box/model.sdf')
        self.f = open(self.modelPath, 'r')
        self.model = self.f.read()
        self.stage = rospy.get_param('/stage_number')
        self.goal_position = Pose()

        self.modelName = 'goal'
        self.obstacle_1 = 1.5, 1.5
        self.obstacle_2 = 1.5, 0
        self.obstacle_3 = 1.5, -1.5
        self.obstacle_4 = 0, -1.5
        self.obstacle_5 = -1.5, -1.5
        self.obstacle_6 = -1.5, 0
        self.obstacle_7 = -1.5, 1.5
        self.obstacle_8 = 0, 1.5

        self.init_goal_x = 0
        self.init_goal_y = 0
        obstacle_true=True

        self.init_goal_x = random.randrange(-10, 10) / 10.0
        self.init_goal_y = random.randrange(-10, 10) / 10.0
            # if abs(self.init_goal_x)<1.2 and abs(self.init_goal_y)<1.2:
            #     obstacle_true=False
            # elif abs(self.init_goal_x)<1.8 and abs(self.init_goal_y)<1.8:
            #     obstacle_true=False
        self.goal_position.position.x = self.init_goal_x
        self.goal_position.position.y = self.init_goal_y
        self.last_goal_x = self.init_goal_x
        self.last_goal_y = self.init_goal_y
        self.last_index = 0
        self.sub_model = rospy.Subscriber('gazebo/model_states', ModelStates, self.checkModel)
        self.check_model = False
        self.index = 0
        self.goal_number = 200
        self.factor = 0

    def checkModel(self, model):
        self.check_model = False
        for i in range(len(model.name)):
            if model.name[i] == "goal":
                self.check_model = True

    def respawnModel(self):
        while True:
            if not self.check_model:
                rospy.wait_for_service('gazebo/spawn_sdf_model')
                spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
                spawn_model_prox(self.modelName, self.model, 'robotos_name_space', self.goal_position, "world")
                rospy.loginfo("Goal position : %.1f, %.1f", self.goal_position.position.x,
                              self.goal_position.position.y)
                break
            else:
                pass

    def deleteModel(self):
        while True:
            if self.check_model:
                rospy.wait_for_service('gazebo/delete_model')
                del_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
                del_model_prox(self.modelName)
                break
            else:
                pass

    def getPosition(self, position_check=False, delete=False):
        if delete:
            self.deleteModel()
            # if self.goal_number < 100:
            #     goal_x = random.randrange(-10, 10) / 10.0
            #     goal_y = random.randrange(-10, 10) / 10.0
            #     position_check = False
            #     self.goal_number += 1
            # else:
            #     while position_check:
            #         goal_x = random.randrange(-26, 26) / 10.0
            #         goal_y = random.randrange(-26, 26) / 10.0
            #         if abs(goal_x - self.obstacle_1[0]) <= 0.55 and abs(goal_y - self.obstacle_1[1]) <= 0.55:
            #             position_check = True
            #         elif abs(goal_x - self.obstacle_2[0]) <= 0.55 and abs(goal_y - self.obstacle_2[1]) <= 0.55:
            #             position_check = True
            #         elif abs(goal_x - self.obstacle_3[0]) <= 0.55 and abs(goal_y - self.obstacle_3[1]) <= 0.55:
            #             position_check = True
            #         elif abs(goal_x - self.obstacle_4[0]) <= 0.55 and abs(goal_y - self.obstacle_4[1]) <= 0.55:
            #             position_check = True
            #         elif abs(goal_x - self.obstacle_5[0]) <= 0.55 and abs(goal_y - self.obstacle_5[1]) <= 0.55:
            #             position_check = True
            #         elif abs(goal_x - self.obstacle_6[0]) <= 0.55 and abs(goal_y - self.obstacle_6[1]) <= 0.55:
            #             position_check = True
            #         elif abs(goal_x - self.obstacle_7[0]) <= 0.55 and abs(goal_y - self.obstacle_7[1]) <= 0.55:
            #             position_check = True
            #         elif abs(goal_x - self.obstacle_8[0]) <= 0.55 and abs(goal_y - self.obstacle_8[1]) <= 0.55:
            #             position_check = True
            #         else:
            #             position_check = False
            #             self.goal_number += 1
            position_check = True
            while position_check:
                # stage 2

                goal_x = random.randrange(-10, 10) / 10.0 * self.factor
                goal_y = random.randrange(-10, 10) / 10.0 * self.factor
                if abs(goal_x - self.obstacle_1[0]) <= 0.55 and abs(goal_y - self.obstacle_1[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x - self.obstacle_2[0]) <= 0.55 and abs(goal_y - self.obstacle_2[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x - self.obstacle_3[0]) <= 0.55 and abs(goal_y - self.obstacle_3[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x - self.obstacle_4[0]) <= 0.55 and abs(goal_y - self.obstacle_4[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x - self.obstacle_5[0]) <= 0.55 and abs(goal_y - self.obstacle_5[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x - self.obstacle_6[0]) <= 0.55 and abs(goal_y - self.obstacle_6[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x - self.obstacle_7[0]) <= 0.55 and abs(goal_y - self.obstacle_7[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x - self.obstacle_8[0]) <= 0.55 and abs(goal_y - self.obstacle_8[1]) <= 0.55:
                    position_check = True
                elif abs(goal_x) < 0.2 and abs(goal_y) < 0.2:
                    position_check = True
                else:
                    position_check = False
                    self.goal_number += 1
                if self.goal_number >= 200:
                    self.factor = 2.6
                else:
                    self.factor = (float(self.goal_number)/float(200))*1.6 + 1

            self.goal_position.position.x = goal_x
            self.goal_position.position.y = goal_y


        self.respawnModel()

        self.last_goal_x = self.goal_position.position.x
        self.last_goal_y = self.goal_position.position.y

        return self.goal_position.position.x, self.goal_position.position.y
